using System;
using System.Collections.Generic;
using System.Text;
using System.Diagnostics;
using Microsoft.Xna.Framework;

namespace Oops.Xna.Framework.Physics.CollisionDetectors
{
	/// <summary>
	/// Detects collisions between two <see cref="CollisionSphere"/> instances.
	/// </summary>
	public class SphereSphereCollisionDetector : CollisionDetector
	{
		#region CollisionDetector Members

		public override void Detect(CollisionShape shape0, CollisionShape shape1, CollisionContext context)
		{
			// Typecast the shapes to their correct subclasses.
			CollisionSphere sphere0 = (CollisionSphere)shape0;
			CollisionSphere sphere1 = (CollisionSphere)shape1;

			// Determine the vector between the spheres.
#if INLINE
#if XBOX
			Vector3 difference = Vector3.Zero;
#else
			Vector3 difference;
#endif
			difference.X = sphere0.Position.X - sphere1.Position.X;
			difference.Y = sphere0.Position.Y - sphere1.Position.Y;
			difference.Z = sphere0.Position.Z - sphere1.Position.Z;

			// Determine the distance of the vector, squared.
			float distanceSquared = difference.X * difference.X + difference.Y * difference.Y + difference.Z * difference.Z;
#elif REFERENCE
#if XBOX
			Vector3 difference = Vector3.Zero;
#else
			Vector3 difference;
#endif
			Vector3.Subtract(ref sphere0.Position, ref sphere1.Position, out difference);

			// Determine the distance of the vector, squared.
			float distanceSquared = difference.LengthSquared();
#else
			Vector3 difference = sphere0.Position - sphere1.Position;

			// Determine the distance of the vector, squared.
			float distanceSquared = difference.LengthSquared();
#endif
			// Get radius totals.
			float radiusTotal = sphere0.Radius + sphere1.Radius;
			float radiusToleranceTotal = radiusTotal + context.Tolerance;

			// Determine if the spheres are close enough for a collision.
			if (distanceSquared >= radiusToleranceTotal * radiusToleranceTotal)
			{
				return;
			}

			// Get the distance between the two spheres' centers.
			float distance = (float)Math.Sqrt(distanceSquared);
			if (distance <= 0.0f)
			{
				return;
			}

			// Calculate the normal of the collision.
#if INLINE
			float inverseDistance = 1.0f / distance;
			difference.X = difference.X * inverseDistance;
			difference.Y = difference.Y * inverseDistance;
			difference.Z = difference.Z * inverseDistance;
#elif REFERENCE
			Vector3.Divide(ref difference, distance, out difference);
#else
			difference /= distance;
#endif
			// Calculate the depth of the penetration due to the collision.
			float collisionDepth = radiusTotal - distance;
            
			// Calculate the position of the collision.
#if INLINE
			float t = sphere1.Radius - (collisionDepth * 0.5f);
			float x = sphere1.Position.X + difference.X * t;
			float y = sphere1.Position.Y + difference.Y * t;
			float z = sphere1.Position.Z + difference.Z * t;
#elif REFERENCE
			Vector3 collisionPosition;
			Vector3.Multiply(ref difference, sphere1.Radius - (collisionDepth * 0.5f), out collisionPosition);
			Vector3.Add(ref sphere1.Position, ref collisionPosition, out collisionPosition);
#else
			Vector3 collisionPosition = sphere1.Position + difference * (sphere1.Radius - collisionDepth * 0.5f);
#endif
			// Create the collision point.
			CollisionPoint point = CollisionPoint.Retrieve();
#if INLINE
			point.RelativePosition0.X = x - shape0.CollisionGroup.Position.X;
			point.RelativePosition0.Y = y - shape0.CollisionGroup.Position.Y;
			point.RelativePosition0.Z = z - shape0.CollisionGroup.Position.Z;

			point.RelativePosition1.X = x - shape1.CollisionGroup.Position.X;
			point.RelativePosition1.Y = y - shape1.CollisionGroup.Position.Y;
			point.RelativePosition1.Z = z - shape1.CollisionGroup.Position.Z;
#elif REFERENCE
			Vector3.Subtract(ref collisionPosition, ref shape0.CollisionGroup.Position, out point.RelativePosition0);
			Vector3.Subtract(ref collisionPosition, ref shape1.CollisionGroup.Position, out point.RelativePosition1);
#else
			point.RelativePosition0 = collisionPosition - shape0.CollisionGroup.Position;
			point.RelativePosition1 = collisionPosition - shape1.CollisionGroup.Position;
#endif
			point.Depth = collisionDepth;

			// Add the collision point to the collection.
			CollisionPointCollection collisionPoints = CollisionPointCollection.Retrieve();
			collisionPoints.Add(point);

			// Notify subscribers of the collision.
			context.OnCollided(shape0, shape1, ref difference, collisionPoints);

			// Recycle the collision point collection.
			CollisionPointCollection.Recycle(collisionPoints);
		}

		#endregion
	}
}
